// 坐标转换工具与机器人状态处理（从 Python 逻辑移植为 JS）

export class CoordinateTransformer {
	constructor() {
		this.map_origin_x = 0;
		this.map_origin_y = 0;
		this.resolution = 0.05;
		this.display_scale = 5; // 显示比例（5倍大小）
		this.width = 0;
		this.height = 0;
		this.display_width = 0;
		this.display_height = 0;
	}

	/**
	 * 更新地图参数
	 * @param {number} width
	 * @param {number} height
	 * @param {number} resolution
	 * @param {number} origin_x
	 * @param {number} origin_y
	 */
	updateParams(width, height, resolution, origin_x, origin_y) {
		this.width = width;
		this.height = height;
		this.resolution = resolution;
		this.map_origin_x = origin_x;
		this.map_origin_y = origin_y;

		// 计算显示尺寸（5倍大小）
		this.display_width = width * this.display_scale;
		this.display_height = height * this.display_scale;
	}

	/**
	 * 将地图坐标转换为显示坐标
	 * @param {number} map_x
	 * @param {number} map_y
	 * @returns {[number, number]} [display_x, display_y]
	 */
	mapToDisplay(map_x, map_y) {
		// 1. 将地图坐标转换为像素坐标
		const pixel_x = (map_x - this.map_origin_x) / this.resolution;
		const pixel_y = (map_y - this.map_origin_y) / this.resolution;

		// 2. 调整到显示坐标系
		const display_x = pixel_x + (this.display_width - this.width) / 2;
		const display_y = pixel_y + (this.display_height - this.height) / 2;

		return [display_x, display_y];
	}

	/**
	 * 处理激光雷达数据（减少数据点）
	 * Python 的 process_laser_scan 的 JS 版本
	 * @param {{ hed:number[], ranges:number[], pose:{x:number,y:number,yaw:number} }} scanData
	 * @param {number} step 采样步长（每隔 step 个点取一个）
	 * @param {number} minRange 最小有效距离（米），小于此值的点将被忽略
	 * @returns {{x:number,y:number}[]} 显示坐标系下的激光点数组
	 */
	processLaserScan(scanData, step = 8, minRange = 0.1) {
		if (!scanData) {
			return [];
		}

		const angle_min = Number(scanData.hed[0] || 0);
		const angle_increment = Number(scanData.hed[2] || 0);
		const ranges = scanData.ranges;
		const scan_pose = scanData.pose || { x: 0, y: 0, yaw: 0 };

		const points = [];
		for (let i = 0; i < ranges.length; i += step) {
			const r = Number(ranges[i] || 0);
			if (r <= minRange) continue;

			// 注意 Python 中是 angle = angle_min + 2 * i * angle_increment
			const angle = angle_min + 2 * i * angle_increment;
			const point_x = r * Math.cos(angle);
			const point_y = r * Math.sin(angle);

			// 转换到世界坐标系
			const world_x = point_x * Math.cos(scan_pose.yaw) - point_y * Math.sin(scan_pose.yaw) + Number(scan_pose.x || 0);
			const world_y = point_x * Math.sin(scan_pose.yaw) + point_y * Math.cos(scan_pose.yaw) + Number(scan_pose.y || 0);

			// 转换到显示坐标系
			const [display_x, display_y] = this.mapToDisplay(world_x, world_y);
			points.push({ x: display_x, y: display_y });
		}

		return points;
	}
}

/**
 * 处理机器人状态数据（从 MQTT 消息中提取并转换）
 * 对应 Python 代码片段的 JS 版本。
 * @param {object} data 来自 MQTT 的消息体（包含 pose 与 robot 字段）
 * @param {CoordinateTransformer} transformer 用于坐标转换的实例
 * @returns {{ robotDisplay: {x:number,y:number,yaw:number,status:number,power:number,velocity:{vx:number,vy:number,vz:number}}, world:{x:number,y:number,yaw:number} }}
 */
export function computeRobotStateUpdate(data, transformer) {
	const pose = data?.pose || {};
	const robot = data?.robot || {};

	// 转换机器人位置到显示坐标系
	const [robot_x, robot_y] = transformer.mapToDisplay(
		Number(pose.x || 0),
		Number(pose.y || 0)
	);

	// 原始世界坐标用于轨迹录制
	const world_x = Number(pose.x || 0);
	const world_y = Number(pose.y || 0);
	const world_yaw = Number(pose.yaw || 0);

	return {
		robotDisplay: {
			x: robot_x,
			y: robot_y,
			yaw: world_yaw * 57.3, // 弧度转角度
			status: Number(robot.status || 0),
			power: Number(robot.power || 0),
			velocity: {
				vx: Number(robot.vx || 0),
				vy: Number(robot.vy || 0),
				vz: Number(robot.vz || 0)
			}
		},
		world: { x: world_x, y: world_y, yaw: world_yaw }
	};
}


